// theta_solver_nlopt.cpp

#include <iostream>
#include <vector>
#include <cmath>
#include <nlopt.hpp>
#include <algorithm>
#include <random>

class ThetaSolver {
public:
    ThetaSolver(double l1_, double l2_, double l3_)
        : l1(l1_), l2(l2_), l3(l3_) {}

    void set_parameters(double arfa_, double bita_, double gama_,
                        double x0_, double y0_, double z0_) {
        arfa = arfa_;
        bita = bita_;
        gama = gama_;
        x0_val = x0_;
        y0_val = y0_;
        z0_val = z0_;
    }

    std::vector<std::vector<double>> solve_all(const std::vector<double> &lb,const std::vector<double> &ub,int num_attempts = 50){
        std::vector<std::vector<double>> solutions;
        std::vector<double> costs;

        nlopt::opt opt(nlopt::GN_CRS2_LM, 3);
        opt.set_min_objective(ThetaSolver::constraint_cost_adapter, this);

        // std::vector<double> lb = {0, 0, 0};
        // std::vector<double> ub = {2 * M_PI, 2 * M_PI, 2 * M_PI};
        opt.set_lower_bounds(lb);
        opt.set_upper_bounds(ub);

        opt.set_xtol_rel(1e-6);
        opt.set_ftol_rel(1e-6);
        opt.set_maxeval(10000);

        std::random_device rd;
        std::mt19937 gen(rd());
        std::uniform_real_distribution<> dis(0.0, 2 * M_PI);

        for (int attempt = 0; attempt < num_attempts; ++attempt) {
            std::vector<double> theta = {dis(gen), dis(gen), dis(gen)};
            double minf;

            try {
                nlopt::result result = opt.optimize(theta, minf);

                if (result >= 0 && minf < 0.1 && is_new_solution(theta, solutions)) {
                    solutions.push_back(theta);
                    costs.push_back(minf);
                }
            } catch (...) {
                // 忽略错误
            }
        }

        return solutions;
    }

private:
    double l1, l2, l3;
    double arfa, bita, gama;
    double x0_val, y0_val, z0_val;
    const double tolerance = 1e-3;

    static double constraint_cost_adapter(const std::vector<double>& theta, std::vector<double>& grad, void* data) {
        return static_cast<ThetaSolver*>(data)->constraint_cost(theta, grad);
    }

    double constraint_cost(const std::vector<double>& theta, std::vector<double>& grad) {
        double t1 = theta[0];
        double t2 = theta[1];
        double t3 = theta[2];

        double m11 = arfa;
        double m12 = x0_val + l1 * cos(t1) * cos(t2);
        double m13 = cos(t1) * cos(t3 - t2);

        double m21 = bita;
        double m22 = y0_val + l1 * sin(t1) * cos(t2);
        double m23 = sin(t1) * cos(t3 - t2);

        double m31 = gama;
        double m32 = z0_val - l1 * sin(t2);
        double m33 = sin(t3 - t2);

        double detM = m11 * (m22 * m33 - m23 * m32)
                    - m12 * (m21 * m33 - m23 * m31)
                    + m13 * (m21 * m32 - m22 * m31);

        double Ax = x0_val + l1 * cos(t1) * cos(t2) - l2 * cos(t1) * cos(t3 - t2);
        double Ay = y0_val + l1 * sin(t1) * cos(t2) - l2 * sin(t1) * cos(t3 - t2);
        double Az = z0_val - l1 * sin(t2) - l2 * sin(t3 - t2);
        double A_sq = Ax * Ax + Ay * Ay + Az * Az;

        double c1 = detM;
        double c2 = A_sq - l3 * l3;

        return c1 * c1 + c2 * c2;
    }

    bool is_new_solution(const std::vector<double>& new_sol, const std::vector<std::vector<double>>& solutions) {
        for (const auto& sol : solutions) {
            double diff = 0.0;
            for (int i = 0; i < 3; ++i) {
                diff += std::abs(sol[i] - new_sol[i]);
            }
            if (diff < tolerance) {
                return false;
            }
        }
        return true;
    }
};

// int main() {
//     ThetaSolver solver(0.275, 0.335, 0.150);
//     solver.set_parameters(0.490772, -0.846193, -0.207606,
//                           0.398544, 0.255645, 0.248709);

//     auto solutions = solver.solve_all(50);

//     int count = 1;
//     for (const auto& theta : solutions) {
//         std::cout << "解 #" << count++ << ":\n";
//         std::cout << "theta1 = " << theta[0] << " rad (" << theta[0] * 180.0 / M_PI << " deg)\n";
//         std::cout << "theta2 = " << theta[1] << " rad (" << theta[1] * 180.0 / M_PI << " deg)\n";
//         std::cout << "theta3 = " << theta[2] << " rad (" << theta[2] * 180.0 / M_PI << " deg)\n\n";
//     }

//     std::cout << "共找到 " << solutions.size() << " 个唯一解。\n";
//     return 0;
// }
